#pragma once
#include <cmath>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <regex>
#include <iomanip>
#include <limits>
#include <cstdlib>   // getenv
#include <cctype>    // isalpha

#include "struct_resource.h"
#include <eigen/Eigen/Dense>
#include "synth.h"

// =====================================================
// 数据目录解析：默认 data/ ，可用环境变量 TIMESYNC_DATA_DIR 覆盖
// =====================================================
inline std::string tsync_data_dir_() {
  const char* envp = std::getenv("TIMESYNC_DATA_DIR");
  if (envp && *envp) return std::string(envp);
  return "data"; // 默认目录
}

// 若 path 是纯文件名（不含分隔符/盘符），自动前置到数据目录
inline std::string resolve_data_path_(const std::string& path) {
  if (path.empty()) return path;

  // 含目录分隔符 → 保持原样
  if (path.find('/') != std::string::npos || path.find('\\') != std::string::npos)
    return path;

#if defined(_WIN32)
  // Windows 盘符（如 C:\ 或 C:）
  if (path.size() >= 2 && std::isalpha(static_cast<unsigned char>(path[0])) && path[1] == ':')
    return path;
#endif

  // 绝对路径（Linux）
  if (!path.empty() && path[0] == '/') return path;

  // 纯文件名：拼接 data/
  std::string base = tsync_data_dir_();
  if (!base.empty() && base.back() != '/' && base.back() != '\\') base += '/';
  return base + path;
}

// =====================================================
// 写入 JSON: { "num":[...], "den":[...], "gamma": <可选> }
// =====================================================
inline bool save_kz_json(const TF& kz,
                         const std::string& path,
                         double gamma = std::numeric_limits<double>::quiet_NaN(),
                         int precision = 17)
{
  if (kz.num.empty() || kz.den.empty()) return false;

  const std::string real_path = resolve_data_path_(path);
  std::ofstream ofs(real_path, std::ios::out | std::ios::trunc);
  if (!ofs) return false;

  ofs.setf(std::ios::fmtflags(0), std::ios::floatfield);
  ofs << std::setprecision(precision);

  ofs << "{ \"num\":[";
  for (size_t i = 0; i < kz.num.size(); ++i) {
    if (i) ofs << ",";
    ofs << kz.num[i];
  }
  ofs << "], \"den\":[";
  for (size_t i = 0; i < kz.den.size(); ++i) {
    if (i) ofs << ",";
    ofs << kz.den[i];
  }
  ofs << "]";

  if (!std::isnan(gamma)) {
    ofs << ", \"gamma\": " << gamma;
  }
  ofs << " }\n";
  return ofs.good();
}

// =====================================================
// 简易解析（针对本项目生成的 JSON 结构）
// =====================================================
inline std::string read_all_text_(const std::string& path) {
  const std::string real_path = resolve_data_path_(path);
  std::ifstream ifs(real_path);
  std::ostringstream ss;
  ss << ifs.rdbuf();
  return ss.str();
}
inline void trim_(std::string& s) {
  size_t b = s.find_first_not_of(" \t\r\n");
  size_t e = s.find_last_not_of(" \t\r\n");
  if (b == std::string::npos) { s.clear(); return; }
  s = s.substr(b, e - b + 1);
}
inline bool parse_array_(const std::string& text, const std::string& key, std::vector<double>& out) {
  std::regex re("\"" + key + "\"\\s*:\\s*\\[([^\\]]*)\\]");
  std::smatch m;
  if (!std::regex_search(text, m, re)) return false;
  std::string inner = m[1].str();
  std::stringstream ss(inner);
  std::string tok;
  out.clear();
  while (std::getline(ss, tok, ',')) {
    trim_(tok);
    if (tok.empty()) continue;
    try { out.push_back(std::stod(tok)); }
    catch (...) { return false; }
  }
  return !out.empty();
}
inline bool parse_number_(const std::string& text, const std::string& key, double& val) {
  std::regex re("\"" + key + "\"\\s*:\\s*([-+0-9eE\\.]+)");
  std::smatch m;
  if (!std::regex_search(text, m, re)) return false;
  try { val = std::stod(m[1].str()); } catch (...) { return false; }
  return true;
}

// 从 JSON 读回 K(z)；若提供 gamma_out 则尝试解析 "gamma"
inline bool load_kz_json(const std::string& path, TF& out, double* gamma_out = nullptr) {
  std::string text = read_all_text_(path);
  if (text.empty()) return false;

  TF tmp;
  if (!parse_array_(text, "num", tmp.num)) return false;
  if (!parse_array_(text, "den", tmp.den)) return false;

  if (gamma_out) {
    double g;
    if (parse_number_(text, "gamma", g)) *gamma_out = g;
    else *gamma_out = std::numeric_limits<double>::quiet_NaN();
  }

  out = std::move(tmp);
  return true;
}

// =====================================================
// TF(z^{-1}) -> StateSpace（可控规范形，den[0] 归一化为 1）
// =====================================================
inline StateSpace tf_to_ss_proper(const TF& H_in) {
  if (H_in.den.empty()) throw std::runtime_error("tf_to_ss_proper: den empty");
  TF H = H_in;

  // 归一化：den[0] = 1
  const double d0 = H.den[0];
  if (std::abs(d0) <= 1e-15) throw std::runtime_error("tf_to_ss_proper: den[0]==0");
  if (std::abs(d0 - 1.0) > 1e-12) {
    for (double& a : H.den) a /= d0;
    for (double& b : H.num) b /= d0;
  }

  const int n = (int)H.den.size() - 1;       // 阶数
  const int m = (int)H.num.size() - 1;
  if (n < 0) throw std::runtime_error("tf_to_ss_proper: invalid den");
  if (m > n) throw std::runtime_error("tf_to_ss_proper: improper TF (deg num > deg den)");

  StateSpace S;
  if (n == 0) { // 纯静态
    S.A.resize(0,0);
    S.B.resize(0,1);
    S.C.resize(1,0);
    S.D = Eigen::MatrixXd::Constant(1,1, H.num.empty()? 0.0 : H.num[0]);
    return S;
  }

  // 填满 num 至长度 n+1（z^{-1} 形式：b0..bn）
  std::vector<double> num = H.num;
  num.resize(n+1, 0.0);

  // A：z^{-1} 可控规范形
  S.A = Eigen::MatrixXd::Zero(n,n);
  for (int i=0; i<n-1; ++i) S.A(i,i+1) = 1.0;
  for (int j=0; j<n;   ++j) S.A(n-1,j) = -H.den[j+1];   // -a1..-an

  // B
  S.B = Eigen::VectorXd::Zero(n);
  S.B(n-1) = 1.0;
  S.B.resize(n,1);

  // C, D
  const double D0 = num[0];             // b0
  S.C = Eigen::RowVectorXd(n);
  for (int i=0; i<n; ++i) {
    const double ai = H.den[i+1];       // a1..an
    const double bi = num[i+1];         // b1..bn
    S.C(i) = bi - ai * D0;
  }
  S.C.resize(1,n);
  S.D = Eigen::MatrixXd::Constant(1,1, D0);
  return S;
}

// =====================================================
// StateSpace -> TF(z^{-1}) （Faddeev–Leverrier）
// =====================================================
inline TF ss_to_tf_zinv(const StateSpace& S) {
  const int nx = (int)S.A.rows();
  // 仅支持 SISO
  if (S.C.rows()!=1 || S.B.cols()!=1 || S.D.rows()!=1 || S.D.cols()!=1)
    throw std::runtime_error("ss_to_tf_zinv: only SISO supported");

  TF H;
  if (nx == 0) {
    H.den = {1.0};
    H.num = { S.D.size()? S.D(0,0) : 0.0 };
    return H;
  }

  Eigen::MatrixXd A = S.A;
  Eigen::VectorXd B = S.B;        // n×1
  Eigen::RowVectorXd C = S.C;     // 1×n
  const double D = S.D(0,0);

  // Faddeev–Leverrier
  const int n = nx;
  std::vector<double> c(n+1, 0.0);                // c1..cn
  std::vector<Eigen::MatrixXd> Sks; Sks.reserve(n);

  Eigen::MatrixXd Sk = Eigen::MatrixXd::Identity(n,n);  // S_0 = I
  Sks.push_back(Sk);
  for (int k=1; k<=n; ++k) {
    Sk = A * Sk + ((k==1)? 0.0 : c[k-1]) * Eigen::MatrixXd::Identity(n,n); // S_k
    c[k] = - (Sk.trace()) / (double)k;                                     // c_k
    if (k < n) Sks.push_back(Sk);
  }

  // den(z)（降幂系数）：z^n + c1 z^{n-1} + ... + cn
  std::vector<double> den_desc(n+1, 0.0);
  den_desc[0] = 1.0;
  for (int k=1; k<=n; ++k) den_desc[k] = c[k];

  // num(z) = D*den(z) + C * adj(zI-A) * B
  // adj(zI-A) = sum_{k=0}^{n-1} S_k z^{n-1-k}
  std::vector<double> num_desc(n+1, 0.0);
  for (int i=0; i<=n; ++i) num_desc[i] += D * den_desc[i];
  for (int k=0; k<=n-1; ++k) {
    double gk = (C * (Sks[k] * B))(0,0);
    num_desc[1 + k] += gk; // 对应 z^{n-1-k}
  }

  // 转 z^{-1} 形式
  H.den.resize(n+1);
  H.num.resize(n+1);
  for (int i=0; i<=n; ++i) {
    H.den[i] = den_desc[i];
    H.num[i] = num_desc[i];
  }

  auto trim_tiny = [](std::vector<double>& v){
    while (v.size()>1 && std::abs(v.back()) < 1e-15) v.pop_back();
  };
  trim_tiny(H.num);
  trim_tiny(H.den);

  // 确保 den[0] = 1
  if (std::abs(H.den[0]) <= 1e-15) throw std::runtime_error("ss_to_tf_zinv: den[0]==0");
  if (std::abs(H.den[0] - 1.0) > 1e-12) {
    const double a0 = H.den[0];
    for (double& x : H.den) x /= a0;
    for (double& x : H.num) x /= a0;
  }
  return H;
}
